// Have include guard.
#ifndef FPK_ENCODERMOTORCONTROLLER_HPP
#define FPK_ENCODERMOTORCONTROLLER_HPP

// Include files.
#include <boost/smart_ptr/shared_ptr.hpp>
#include "WPILib.h"
#include "PidMotorController.hpp"

/**
 *  The EncoderMotorController class pairs a SpeedController and an Encoder into a PidMotorController so FPK can use PID-dependent features like Speed and Position/Distance control.
 */
class EncoderMotorController :
	public PidMotorController, public LiveWindowSendable
{
	public:
		EncoderMotorController(boost::shared_ptr<SpeedController> motor, boost::shared_ptr<Encoder> encoder, float period = 0.05);
		EncoderMotorController(boost::shared_ptr<SpeedController> motor, UINT32 encoderChannelA, UINT32 encoderChannelB, bool encoderReverseDirection = false, float period = 0.05);
		EncoderMotorController(SpeedController *motor, Encoder *encoder, float period = 0.05);
		EncoderMotorController(SpeedController *motor, UINT32 encoderChannelA, UINT32 encoderChannelB, bool encoderReverseDirection = false, float period = 0.05);
		EncoderMotorController(SpeedController &motor, Encoder &encoder, float period = 0.05);
		EncoderMotorController(SpeedController &motor, UINT32 encoderChannelA, UINT32 encoderChannelB, bool encoderReverseDirection = false, float period = 0.05);
		
		virtual void Set(float value, UINT8 syncGroup = 0);
		virtual float Get();
		
		virtual double GetVBus();
		virtual double GetSpeed();
		virtual double GetPosition();
		virtual void Enable(double initialEncoderPosition);
		virtual void Disable();
		virtual ControlMode GetControlMode();
		virtual void SetControlMode(ControlMode mode);
		virtual void SetEncodersCodesPerRevolution(UINT16 codesPerRevolution);
		virtual void SetPid(double p, double i, double d);
		virtual double GetP();
		virtual double GetI();
		virtual double GetD();
		
		virtual void InitTable(ITable* subtable);
		virtual ITable* GetTable();
		virtual std::string GetSmartDashboardType();
		
		virtual void UpdateTable();
		virtual void StartLiveWindowMode();
		virtual void StopLiveWindowMode();
	private:
		typedef double(*PidSourceFunction)(EncoderMotorController *motor);
		
		static double GetVBus(EncoderMotorController *motor);
		static double GetSpeed(EncoderMotorController *motor);
		static double GetPosition(EncoderMotorController *motor);
		static void ResetTimer(void *pseudoCanJaguar);
		
		boost::shared_ptr<SpeedController> m_motor;
		boost::shared_ptr<Encoder> m_encoder;
		float m_period;
		Notifier m_notifier;
		PIDController m_pid;
		Timer m_timer;
		double m_speed;
		double m_offset;
		ControlMode m_controlMode;
		PidSourceFunction m_sourceFunction;
		
		void Initialize();
};

#endif // #ifndef FPK_ENCODERMOTORCONTROLLER_HPP
